#!/usr/bin/env python

#Opendog Gait testing
#developed by SL March.2020


from numpy import *
import time
import Queue   # might not be needed

#basics
import rospy
import sys
import roslib
import math
roslib.load_manifest('odrive_ros')


# Imports message type
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Float64


class FootPath:
	def __init__(self):

		self.foot_position = rospy.get_param('~foot_position', "/footPosition_1")
		self.phase_shift = rospy.get_param('~phase_shift', 0.0)

		# define parameters for sinusoidal path

		#parameters for forward walking gait
		self.leg_pace = 2.0 # pace of gait
		self.x_center_foward = -0.5
		self.x_stride_forward = -4.0
		self.y_center_forward = 25.0
		self.y_lift_forward = 3.0


		self.x_center_turn = -0.5
		self.x_stride_turn = -4.0
		self.y_center_turn = 25.0
		self.y_lift_turn = 3.0

		self.x_center_swivel = -0.5
		self.x_stride_swivel = -4.0
		self.y_center_swivel = 25.0
		self.y_lift_swivel = 3.0

		self.x_center_side = -0.5
		self.x_stride_side = -4.0
		self.y_center_side = 25.0
		self.y_lift_side = 3.0

		# Initialize "current" values
		self.ynow,self.tnow = 0.0, 0.0
		self.xnow,self.znow = 0.0, 0.0


		# Set up a publisher
		self.pub = rospy.Publisher(self.foot_position, PoseStamped, queue_size = 1)

		# Set up a timed loop
		rospy.Timer(rospy.Duration(0.02), self.timer_callback, oneshot=False)

		rospy.Service('Foward_walking', std_srvs.srv.Trigger, self.Forward_walking)
		rospy.Service('Turn', std_srvs.srv.Trigger, self.Turn)
		rospy.Service('Swivel', std_srvs.srv.Trigger, self.Swivel)
		rospy.Service('Side_walk', std_srvs.srv.Trigger, self.Side_walk)
		rospy.Service('Down', std_srvs.srv.Trigger, self.Down)


	def Forward_walking(self, data, request):
		try:
			#what is the time now?
			# self.tnow = rospy.Time.now()
			self.tnow = time.time()

			# now where should the foot be?
			# rospy.logwarn(self.x_center, self.x_stride, self.leg_pace, self.tnow, self.phase_shift)
			self.xnow_forward = -(self.x_center_forward + self.x_stride_forward*sin(self.leg_pace*self.tnow - self.phase_shift))
			#rospy.logwarn(str(self.xnow) + ' , ' + str(self.ynow))


			self.ynow_forward= (self.y_center_forward + self.y_lift_forward*sin(self.leg_pace*self.tnow - self.phase_shift - pi/2))

			if (self.ynow_forward) > self.y_center_forward: self.ynow _forward =  self.y_center_forward

			# Create and publish PoseStamped message containing the (x,y) position of the foot
			# Eventually will include z when hip motion is included
			footPosition = PoseStamped()
			footPosition.header.stamp = rospy.Time.now()
			footPosition.pose.position.x = self.xnow_forward
			footPosition.pose.position.y = self.ynow_forward
			footPosition.pose.position.z = 0.0
			footPosition.pose.orientation.x = 0.0
			footPosition.pose.orientation.y = 0.0
			footPosition.pose.orientation.z = 0.0
			footPosition.pose.orientation.w = 0.0
			self.pub.publish(footPosition)

			# time.sleep(.01)

		except KeyboardInterrupt:
			#close('all')
			#break
			pass

	def Turn(self, data, request):
		try:
			self.tnow = time.time()
			self.xnow_turn = self.x_center_turn + self.x_stride_turn*sin(self.leg_pace*self.tnow - pi/2 - leg1_offset)
			self.ynow_turn = self.y_center_turn - self.y_offset_turn*sin(self.leg_pace*self.tnow - pi - leg1_offset)
			self.znow_turn = self.z_center_turn + self.z_lift_turn*sin(self.leg_pace*self.tnow - pi/2 - leg1_offset)

			footPosition = PoseStamped()
			footPosition.header.stamp = rospy.Time.now()
			footPosition.pose.position.x = self.xnow_turn
			footPosition.pose.position.y = self.ynow_turn
			footPosition.pose.position.z = self.znow_turn
			footPosition.pose.orientation.x = 0.0
			footPosition.pose.orientation.y = 0.0
			footPosition.pose.orientation.z = 0.0
			footPosition.pose.orientation.w = 0.0
			self.pub.publish(footPosition)
		except KeyboardInterrupt:
			#close('all')
			#break
			pass

	def Swivel(self, data, request):
		try:
			self.tnow = time.time()
			self.xnow_swivel = self.x_center_swivel + self.x_stride_swivel*sin(self.leg_pace*self.time - pi/2 - leg1_offset)
			self.ynow_swivel = self.y_center_swivel - self.y_offset_swivel*sin(self.leg_pace*self.time - pi - leg1_offset)
			self.znow_swivel = self.z_center_swivel + self.z_lift_swivel*sin(self.leg_pace*self.time - pi/2 - leg1_offset)

			footPosition = PoseStamped()
			footPosition.header.stamp = rospy.Time.now()
			footPosition.pose.position.x = self.xnow_swivel
			footPosition.pose.position.y = self.ynow_swivel
			footPosition.pose.position.z = self.znow_swivel
			footPosition.pose.orientation.x = 0.0
			footPosition.pose.orientation.y = 0.0
			footPosition.pose.orientation.z = 0.0
			footPosition.pose.orientation.w = 0.0
			self.pub.publish(footPosition)
		except KeyboardInterrupt:
			#close('all')
			#break
			pass

	def Side_walk(self, data, request):
		try:
			tnow = time.time()
			self.xnow_swivel = self.x_center_swivel + self.x_stride_swivel*sin(self.leg_pace*tnow - pi/2 - leg1_offset)
			self.ynow_swivel = self.y_center_swivel + self.y_offset_swivel*sin(self.leg_pace*tnow - pi - leg1_offset)
			self.znow_swivel = self.z_center_swivel + self.z_lift_swivel*sin(self.leg_pace*tnow - pi/2 - leg1_offset)

			footPosition = PoseStamped()
			footPosition.header.stamp = rospy.Time.now()
			footPosition.pose.position.x = self.xnow_swivel
			footPosition.pose.position.y = self.ynow_swivel
			footPosition.pose.position.z = self.znow_swivel
			footPosition.pose.orientation.x = 0.0
			footPosition.pose.orientation.y = 0.0
			footPosition.pose.orientation.z = 0.0
			footPosition.pose.orientation.w = 0.0

			self.pub.publish(footPosition)
			
		except KeyboardInterrupt:
			#close('all')
			#break
			pass


def main(args):
	rospy.init_node('Gait_testing',anonymous=True)
	FP = FootPath()

	try:
		rospy.spin()
	except KeyboardInterrupt:
		print "shutting down"

if __name__=='__main__':
	main(sys.argv)
